from chart_to_grid import *
from grid_make import GridFiller
import matplotlib.pyplot as plt
from AStar import AStarPlanner
import time


def main():
    color_1 = np.array([219, 235, 182])
    color_2 = np.array([233, 224, 167])
    color_3 = np.array([255, 218, 170])
    color_list = [color_1, color_2, color_3]
    cell_size = 100
    dilation_size = 1
    start_org = [1000, 18000]
    goal_org = [50000, 3000]
    wh = 1.0

    grid_chat = nautical_chart_to_grid("D:\\project23\\nautical_chart\\yima_weihai",
                                       37.5838440, 122.0020346,
                                       37.3934620, 122.6021947, 14,
                                       color_list, 10, cell_size)
    grid_filler = GridFiller(grid_chat, cell_size)

    # 对障碍物进行膨胀处理
    dilated_grid_org = grid_filler.dilate_obstacles(dilation_size)
    dilated_grid = dilated_grid_org.T

    # 创建画布和坐标轴
    fig, ax = plt.subplots(figsize=(9, 9))
    ax.set_xticks(np.arange(0, dilated_grid.shape[1], 1))
    ax.set_yticks(np.arange(0, dilated_grid.shape[0], 1))

    # # 设置坐标轴样式
    ax.set_xticklabels([])
    ax.set_yticklabels([])
    ax.tick_params(length=0)
    plt.gca().set_aspect('equal', adjustable='box')
    plt.grid(True, linestyle='-', linewidth=1)
    plt.title("Astar")

    # 设置障碍物格栅为黑色，背景为白色
    cmap = plt.cm.binary

    # 显示经过膨胀处理的格栅中的多边形
    plt.imshow(dilated_grid, cmap=cmap, origin='lower', extent=[0, dilated_grid.shape[1], 0, dilated_grid.shape[0]])

    # 设置起点和终点，用五角星和六边形表示
    start = (int(start_org[0] / cell_size), int(start_org[1] / cell_size))
    # goal = (int((grid_size[0] - 10) / cell_size), int((grid_size[1] - 50) / cell_size))
    goal = (int(goal_org[0] / cell_size), int(goal_org[1] / cell_size))
    plt.plot(start[0], start[1], marker=(5, 1, 0), markersize=10, color="blue")
    plt.plot(goal[0], goal[1], marker=(5, 1, 0), markersize=10, color="green")

    # 进行A*算法路径规划
    a_star = AStarPlanner(dilated_grid_org, wh)
    # 计算规划时间
    start_time = time.time()
    path, new_path = a_star.planning(start, goal)
    end_time = time.time()
    print("A*算法规划时间：", end_time - start_time)

    # 绘制A*算法规划的路径，用黄色的线表示
    if path is not None and len(path) > 0:
        path = np.array(path)
        plt.plot(path[:, 0], path[:, 1], linewidth=3, color='y')

    # 绘制优化后的路径，用蓝色的线表示
    if new_path is not None and len(new_path) > 0:
        new_path = np.array(new_path)
        plt.plot(new_path[:, 0], new_path[:, 1], linewidth=2, color='b')
        # 将各个点的绘制出来,用绿色的圆点表示
        for i in range(len(new_path)):
            plt.plot(new_path[i][0], new_path[i][1], 'go', markersize=6)

    #保存图片，保存到当前路径下，并以时间戳命名
    plt.savefig(str(time.time()) + '.png')
    plt.show()


if __name__ == '__main__':
    main()
